Systems and methods for image aided navigation

ABSTRACT

In one embodiment, a vision aided navigation system comprises: at least one image sensor configured to produce image frames of a surrounding environment; a feature extractor configured to extract at least one image feature from a first image frame; a navigation filter configured to output a navigation solution based navigation data from a navigation device and changes in position of the image feature in the images; a feature tracker to receive the image frames and predict a location of the image feature in a subsequent image frame; a dynamic localized parameter adjuster to adjust at least one image parameter of the subsequent image frame; and wherein the feature tracker is configured so that when the image feature cannot be identified in the subsequent image frame within a bounded region around the predicted location, the dynamic localized parameter adjuster adjusts the at least one image parameter within the bounded region.

STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT

This invention was made with U.S. Government support underFA8750-16-C-0043 awarded by AFRL and DARPA. The U.S. Government hascertain rights in the invention.

BACKGROUND

Vision based sensors, such as cameras, are often used in conjunctionwith vehicle navigation systems to aid in navigation under circumstanceswhere information received from primary navigation sensors (such asGlobal Navigation Satellite System receivers) may be degraded orunavailable. For example, vision-based sensors can be used to trackfeatures captured in images of the vehicle's surroundings to aid aninertial measurement unit in computing navigation solutions. Further,vision-based sensors can be used to track features in the operatingenvironment of the vehicle to enable navigation, guidance, objectidentification, and other forms of autonomous operation. However, whilesuch vision-based sensors are typically calibrated for operating under apredefined set of environmental conditions (for example, conditions suchas the amount of ambient light and moisture content in the atmosphere),it can be expected that the vehicle will experience dynamicenvironmental conditions that vary over time during vehicle operation.Such changes in environmental conditions can alter the ability of asystem to interpret its environment. For example, the number or qualityof features available for a vision-based system to extract from capturedimages may change with environmental conditions. Extracted featuresinherently provide more useful navigation data the longer they aretracked. As such, when features become obscured or are lost, theirability to aid the navigation system is substantially diminished.

SUMMARY

A vision aided navigation system is provided. The system comprises: atleast one image sensor, wherein the at least one image sensor isconfigured to produce a plurality of image frames of a surroundingenvironment; a feature extractor coupled to the at least one imagesensor, wherein the feature extractor is configured to extract at leastone image feature from a first image frame from the plurality of imageframes captured by the at least one image sensor; a navigation filterconfigured to output a navigation solution based on an input ofnavigation data from at least one navigation device and further based onchanges in position of the at least one image feature in the pluralityof images; a feature tracker configured to receive the plurality ofimage frames and configured to predict a location of the at least oneimage feature in at least one subsequent image frame taken by the atleast one image sensor; a dynamic localized parameter adjusterconfigured to adjust at least one image parameter of the at least onesubsequent image frame; and wherein the feature tracker is configured sothat when the at least one image feature cannot be identified in the atleast one subsequent image frame within a bounded region around thepredicted location, the dynamic localized parameter adjuster adjusts theat least one image parameter within the bounded region.

DRAWINGS

Understanding that the drawings depict only some embodiments and are nottherefore to be considered limiting in scope, the exemplary embodimentswill be described with additional specificity and detail using theaccompanying drawings, in which:

FIGS. 1 and 1A are block diagrams of an example embodiment of a visionaided navigation system of the present disclosure;

FIGS. 2A, 2B and 2C are diagrams illustrating dynamic localized imageparameter adjustments for captured image frames for an example visionaided navigation system; and

FIG. 3 is a flow chart demonstrating an example embodiment of a methodfor vision aided navigation using dynamic localized image parameteradjustments.

In accordance with common practice, the various described features arenot drawn to scale but are drawn to emphasize specific features relevantto the example embodiments.

DETAILED DESCRIPTION

In the following detailed description, reference is made to theaccompanying drawings that form a part hereof, and in which is shown byway of illustration specific illustrative embodiments. However, it is tobe understood that other embodiments may be utilized and that logical,mechanical, and electrical changes may be made.

Embodiments of the present disclosure provides a system and methods thatallow a vehicle's vision-based navigation system to better adapt todynamic environmental conditions by applying targeted image enhancementprocessing to captured image frames when tracked features fail to appearin or near their predicted locations. That is, when a feature isextracted from a given position in a captured image frame, its positionin a subsequently captured second image frame can be accuratelypredicted given estimates of vehicle kinematics (e.g. position,velocity, acceleration and heading) from the navigation system of howthe vehicle is believed to have moved between the time the first andsecond image frames were captured and correspondingly how the featurehas to be scaled, translated and rotated accordingly. If the trackedfeature appears precisely in the second image frame where predicted,then this determination validates that the navigation solutions beinggenerated by the vehicle navigation system are accurate. If the trackedfeature position appears in the second image frame in a location closeto, but offset from, where it was predicted to appear, and/or orienteddifferently from the prediction then this determination indicates thatsome errors may exist in the navigation solutions being generated by thevehicle navigation system, and the detected position and orientationerror may be fed back to the navigation system for corrections to thenavigation estimates. However, when due to changes in operatingenvironment a tracked feature can no longer be identified within abounded region around where the feature was predicted to appear, thenthat feature cannot be reliably used to aid in judging the accuracy ofthe navigation solution from the navigation system. Changes in theoperating environment that could trigger loss of identification oftracked feature from one frame to another could include, but not limitedto, (i) changes in lighting condition impacting captured frame or (ii)motion of the non-stationary object, whose feature is tracked, relativevehicle's motion and/or obstructions in the way of the field of view ofthe camera. With embodiments of the present disclosure, when a trackedfeature is lost from one image frame to the next, image enhancementtechniques are applied to the region of the image from where the trackedfeature was predicted to appear. That is, as opposed to applying imageenhancement to the entire image, image enhancement techniques are onlyapplied a bounded area proximate to where the tracked feature waspredicted to appear. In this way, processing time and resources areconserved. The size and shape of the bounded area may be determinedbased on considerations such as, but not limited to, vehicle speed,stage of operation, the standard deviation of the vehicle positionestimates, or the standard deviation of the tracked feature position.While the region may be optimized to a portion of the image, it isunderstood that the region of the image may encompass the entirety ofthe image. If the tracked feature then appears and becomes extractablefrom the second image due to the adjustments, then its position in thesecond image may be fed back to the navigation system as an aid. If thetracked feature still no longer appears in second image even with theadjustments, the navigation system may then determine whether todiscontinue tracking of that feature. In some examples, a set of trackedfeatures may be dynamically updated adding and removing features over aplurality of image frames. The set of tracked features may be used fornavigation purposes and to better optimize the region.

FIG. 1 is a block diagram of an example embodiment of a vision aidednavigation system 100 for a vehicle 10, according to the presentdisclosure. It should be understood that the vision aided navigationenabled vehicle 100 described through this disclosure is not limited toany one particular type of vehicle, but may be used in conjunction withany form aircraft, automobile, truck, train, watercraft, spacecraft,etc. As shown in FIG. 1 , the vision aided navigation system 100comprises an image sensor 102 (which in some embodiments may be mountedto the vehicle 10), a feature extractor 106, a feature tracker 104, anavigation filter 108, a dynamic localized parameter adjuster 110, and anavigation unit 112. In the example shown in FIG. 1 , the vision aidednavigation system 100 includes a processor 114 coupled to a memory 116and is programmed to execute code for implementing the feature tracker104, the feature extractor 106, and navigation filter 108, dynamiclocalized parameter adjuster 110, and other functions described hereinattributed to the vision aided navigation system 100.

In the example shown in FIG. 1 , the navigation unit 112 is coupled tothe vision aided navigation system 100. In some embodiments, thenavigation unit 112 may use the output navigation solution from thenavigation filter 108 to automate the operation and movements of thevehicle 10. For example, in some embodiments, the navigation unit 112can provide navigation statistics to the vehicle's guidance system forcollision avoidance and/or guide the vehicle to a desired destination.In some examples, the navigation unit 112 in conjunction with theguidance system provides an alarm to the driver of the vehicle 10, or toother components of the vehicle, when a clear and/or unobstructed pathto maneuver the vehicle 10 is not immediately available.

FIG. 1A is a block diagram illustration of one example embodiment of thevision aided navigation system 100. In the embodiment shown in FIG. 1A,the image sensor 102 (which may be mounted to the vehicle 10) is coupledto, and outputs image frames fed to, the feature tracker 104 and thefeature extractor 106. The image sensor 102, in various differentembodiments, may be implemented using any device capable of capturingimages of the physical surroundings of vehicle 10, and outputting imageframes of those captured images. For example, in some embodiments, theimage sensor 102 may comprise a camera or other sensor that works tocapture images in any number of spectrums such as, for example, infrared(IR) visual spectrum, and/or other spectrums of light. The image sensor102 may be used in alternate embodiments where lighting conditions mayvary. For example, the feature tracker 104 and feature extractor 106 cantrack and extract features from a stationary image sensory 102, such asa security camera, tracking motion of features within a still-frame.Alternatively, the feature tracker can be used in conjunction withsatellite imagery as described in other embodiments.

In some embodiments, the image sensor 102 is configured for continuousgeneration of image frames representing sequences of scenes of theenvironment surrounding vehicle 10 as captured by the image sensor 102.As such, each captured image frame can be analyzed by the system 100 inrelation to a previous and/or subsequent image frame captured by theimage sensor 102. Each captured image may be stored in memory 116 orstored in another component. Images generated by the image sensor 102can be accessed by one or more of the feature tracker 104, the featureextractor 106, and/or the parameter adjuster 110.

In some embodiments, each image generated by the image sensor 102 isstored as a two-dimensional image frame comprising a plurality ofidentifiable pixels and known pixel dimensions. The pixels of the imageframe may be referenced to and analyzed using an array. Each pixel maybe identified by the row and column, such that a pixel in one image hasa corresponding pixel in a previous or subsequent image. In otherembodiments, features within the image frame may be identified andtracked using other methods, such as using vector coordinates toidentify the position of features.

Each image frame utilized by the system 100 may capture one or moreimage features that are representations of objects, or elements of suchobject, observed by the image sensor 102. As such, the image features asthey appear in captured image frames will comprise a group of pixelsspatially organized as a geometric entity with sufficientdifferentiating characteristics (such as contrast, for example) relativeto a background such as to be recognizable and distinguishable from thebackground. For example, an object that exists in the environment of thevehicle 10 and is viewable from the image sensor 102 may bedistinguishable from the background. The exact parameters which define afeature as a feature may depend on the implementation of the featureextractor 106. An image feature may be identifiable as a feature in asingle image frame, and it may become a feature that is tracked over aplurality of images.

The feature tracker 104 and feature extractor 106 are each configured toprocess the image frames, discussed above, that are received from theimage sensor 102. The feature extractor 106 processes the image framesto identify collections of pixels that have a sufficient correlation ofdifferentiating characteristics as an image feature to qualify as apotential trackable feature to be tracked by the feature tracker 104over a sequence of image frames. Given the position of an image featurein a first image frame, the feature tracker 104 is configured toestimate (e.g. predict) a position where the tracked image featureshould appear in subsequent image fames, given the distance anddirection traveled by the vehicle 10 in the intervening time. A numberof feature extraction algorithms would be known to those in the art whohave reviewed this disclosure to implement the feature extractor 106.Such examples include but are not limited to the Orientation FAST androtated BRIEF (ORB) algorithm, the Scale-Invariant Feature Transform(SIFT) algorithm, and the Speeded-Up Robust Features (SURF) algorithm,as well as “data driven” ML-model based approaches.

The feature extractor 106 is configured to extract at least one imagefeature from each image frame. In some embodiments, the featureextractor 106 may extract any arbitrary number, N, of features from aframe. Image features can be identified through any known means, such asrandom search or swarm search of image. In some embodiments, the featureextractor 106 takes into consideration the kinematic properties of thevehicle transmitted from the navigation filter 108 to determine whichimage features appearing in an image frame should be extracted. Forexample, when the vehicle 10 is moving straight in an unchangingdirection, the feature extractor 106 may place a higher weight on thefeatures in the center of the image frame, and less weight to theavailable features towards the edge of the image frame. The kinematicproperties, or kinematic states, of the vehicle may include parameterssuch as, but not limited to, the three-dimensional position,three-dimensional velocity, three-dimensional acceleration, tilt, yaw,and any other physical property related to vehicle dynamics.

The feature tracker 104 is configured to generate and store featuretracks for the tracked features. Each feature track comprises a set ofinformation relating to the position and other characteristics for anextracted image feature over time. For example, a feature track maycomprise a set of coordinates associated with the extracted imagefeature appearing in a series of image frames. Other information thatmay be included in the feature track may include, but is not limited to,a relative luminosity and/or a color of the image feature, an estimatedvelocity of the tracked feature, or if a tacked feature disappears fromsubsequent images, the number of subsequent image frames in which thetracked feature could not be identified. In some embodiments, thefeature track for an associated tracked feature may be stored in thememory 116 and accessed by other components within the vision aidednavigation system 100 including the feature extractor, navigation filter108, and dynamic localized parameter adjuster 110.

In some embodiments, the feature track for a tracked object may includea feature vector that indicates the relative motion of the feature withrespect to the vehicle and may include the predicted motion of thefeature. The feature vector may include an approximation of the trackedfeature's movement alone, the motion of the vehicle relative to thefeature, or a combination of the two. The feature vector may contain amean and a variance of the feature's estimated position. The mean andthe variance of the feature's estimated position may be mapped to abounded region in the image. In some embodiments, the feature vector maybe used to estimate the position of the at least one feature in asubsequent image. In some embodiments, the magnitude of the featurevector may be used to determine the size of a bounded area, or boundedregion, associated with a tracked image on subsequent image frames, asfurther discussed below.

The feature tracker 104 may be configured to prioritize a predeterminedset of objects based on the properties of the object. The prioritizationcan be determined apriori and/or determined at a specified runtime(dynamic priority). For example, in one embodiment, the feature tracker104 is configured to prioritize stationary objects, i.e. buildings,trees, or parked cars. When the feature tracker 104 tracks featuresassociated with an object moving independently from the vehicle 10, i.e.another moving vehicle, that feature is inherently less valuable for thedetermining the future position of the vehicle 10 as the object's motionis independent of the vehicle. For example, if the feature tracker 104tracks a feature on another vehicle traveling at the same speed and inthe same direction as the vehicle 10, then the feature will appear to bestationary when comparing subsequent image frames. If the featuretracker 104 determines that the tracked feature is moving independentlyfrom the vehicle 10, then that feature may be removed as a trackedfeature. In some cases, objects that move independent of the vehicle 10are more likely to quickly exit the field of view of the imaging device102. Thus, these moving features provide less information to thenavigation system 100 as compared to stationary features. A featurebecomes more valuable to the navigation system the longer it is tracked,as generally the predicted feature locations become more accurate. Asthe vehicle moves and feature positions are predicted and measured overtime, the features can also be selected to minimize the dilution ofprecision (DOP), to minimize computation time by selecting features thatare relevant or add new information, and to improve the estimatedstatistics of the vehicle's kinematic state by selecting features thatprovide optimal geometry. The predicted feature positions allow aconcentration on specific regions of the image and the adjustment ofimage parameters to find features in those regions of the image. Aregion may be defined such that the center of the region is the mean ofthe predicted feature location and the shape of the region, such as anellipse, may be derived from the covariance of the estimated featureposition.

The feature tracker 104 sends the predicted location of the imagefeatures being tracked to the feature extractor 106. The featureextractor 106 then searches for those image features within the boundedregions associated with each tracked image feature. In some examples,the feature extractor searches for the same type of feature. In otherexamples, the feature extractor 106 searches for any feature within theenclosed bounded region. The feature tracker 104 attempts to locate inthe present image frame the tracked features that appeared in one ormore of the previous image frames. The feature tracker 104 uses theprevious frame and the locations of the at least one feature from aprevious frame to locate the feature in the subsequent frame. Thefeature tracker 104 may estimate the predicted locations of the trackedfeatures using the feature tracks associated with the features. Forexample, in one embodiment, the feature tracker 104 searches for animage feature within the current image frame around the region of thepredicted location of the feature. If the feature is found, then thefeature tracker 104 updates the location of the feature in thatfeature's feature track in memory 116. In one embodiment, when anextracted feature is identified within the area searched, the featuretracker 104 determines whether that feature is the same feature as atracked feature from a prior image.

In the example shown in FIG. 1 , the navigation filter 108 is coupled tothe feature tracker 104. In some embodiments, the navigation filter 108is also coupled to an inertial measurement unit (IMU) 118 and/or aGlobal Navigation Satellite System (GNSS) receiver 120, and/or othernavigational sensor. In some embodiments, the navigation filter 108receives the feature tracks from the feature tracker 104 correspondingto one or more of the image features being tracked by the featuretracker 104. The navigation filter 108 uses these feature tracks to aidin the prediction of a navigation solution (for example, the positionand velocity of the vehicle 10). In some embodiments, the navigationfilter 108 may be implemented using a Kalman filter or other Bayesianalgorithm. Based on where a tracked feature appears in (or is missingfrom) an image frame where predicted, the navigation filter 108generates a residual, or error state, vector. A low norm of the residualvector indicates that the tracked features appear in the image frameapproximately where expected, which means that the navigation filter'sestimates of the velocity and position of the vehicle 10 (e.g. ascalculated from the IMU 118 and/or GNSS receiver 120 measurements) arelikely accurate. As the position of a tracked feature deviates from theexpected location in an image frame, the norm of the residual vectorgenerated by the navigation filter 108 will increase, which may, amongother things, imply that the navigation filter's estimates of theposition and velocity of the vehicle 10, as calculated from the IMU 118and/or GNSS receiver 120 measurement, are not accurate.

In some embodiments, the navigation filter 108 also provides feedback tothe feature tracker 104 and/or the feature extractor 106. This feedbackmay include approximations of the statistics of the vehicle's kinematicstates (e.g. position and velocity). In some embodiments, the featureextractor 106 uses the feedback from the navigation filter 108 to moreprecisely find and extract features within image frames. In someembodiments, the navigation filter 108 provides the feature tracker 104with the approximations of the statistics of the vehicle's kinematicstates (e.g. position and velocity). The feature tracker 104, in someembodiments, may use the estimated statistics of the vehicle's kinematicstates in predicting the location of tracked features.

As discussed above, with embodiments of the present disclosure, when atracked feature that appeared in prior image frames unexpectedly nolonger can be identified or extracted from a subsequent image frame,image enhancement techniques are applied to the area of the image fromwhere the tracked feature was predicted to appear.

In some embodiments, the feature tracker defines a bounded region in theimage frame around an area that includes the location where each trackedfeature is predicted to appear (that is, the predicted image featurelocation). That is, the bounded region defines a localized area withinimage frame where, based on the tracking of images features from one ormore prior image frames, an image feature is expected to appear. In someembodiments, the size and/or shape of the bounded regions applied by thefeature tracker 104 may be fixed, or predefined. In other embodiments,the size and/or shape of the bounded region may be dynamicallycalculated as a function of an estimated confidence, or covariance, inthe predicted location of the image feature. Where the confidence in thepredicted location is high (the variance of the estimated location islow), the bounded region may be relatively smaller in area as comparedto when the confidence in the predicted location is low (the variance ofthe estimated location is high). In other embodiments, other vehicledynamics may factor into the size of the bounded region, such as vehiclespeed and/or velocity, acceleration, or vibrations (e.g., a smallerbounded region may be utilized for a vehicle travelling at slowervelocity, and a larger bounded may be utilized for a vehicle travellingat a faster velocity). The bounded region need not be limited to anyparticular shape but may be defined using any regular or irregulargeometric shape (e.g., a square, a triangle, an ellipse, etc.). For atleast some embodiments, this bounded region defines a collection ofpixels that may be manipulated by the dynamic localized parameteradjuster 110.

In some embodiments, when a tracked feature cannot be readily identifiedwithin the bounded region in which it is expected to appear, the dynamiclocalized parameter adjuster 110 executes and applies one or more imageenhancement filters (for example, to adjust luminosity, contrast, hues,or other image parameter) to the bounded region associated with themissing tracked feature to attempt to make that feature sufficientlyvisible so that it may be identified and extracted by the featureextractor 106 and/or tracked by the feature tracker 104.

If the feature extractor 106 cannot identify the tracked feature, thenthe dynamic localized parameter adjuster 110 adjusts the parameters ofthe image in the bounded region associated with the missing imagefeature. This process may be repeated a number of times with differentparameters if the prior iteration does not yield a sufficiently visiblefeature. The parameters of the image that can be adjusted by applyingimage correction filters may include, for example luminosity, contrast,hue, saturation, gamma, or apply other image correction filters toaugment the image parameters within the bounded region. Once the imageparameters have been augmented, the feature extractor 106 again searchesthe bounded region to determine if the missing tracked feature is nowextractable from the image frame. If so, then the location of thetracked feature can be added to the feature track and provided to thefeature tracker 104 for processing as described above.

FIGS. 2A, 2B, and 2C illustrate the processing of image frames by thevision aided navigation system 100 in order to enhance the ability ofthe vision aided navigation enabled vehicle 100 to track image featuresas environmental conditions change. Each of the images in FIGS. 2A, 2B,and 2C represent image frames captured by the image sensor 102 as thevehicle 10 is in motion.

FIG. 2A depicts in image frames 200 a, 200 b, and 200 c the operation ofthe vision aided navigation system 100 under optimal lightingconditions. 200 b and 200 c serve to contrast the image frames 201 a and201 b, in FIGS. 2B, and 2C where the same image frames are shown underdifferent environmental conditions. For example, brighter lighting maywash out image frames 201 a and 201 b, making it more difficult toidentify features than in image frames 200 b and 200 c.

Beginning with image frame 200 a in FIG. 2A, this image frame 200 adepicts an image captured under nominal environmental conditions. In theexample, a limited number of image features (shown at 202) have beenidentified in the image frame for extraction by the feature extractor106. These extracted features 202 are from points of the image withidentifiable features. In the particular example 200 a, the identifiableextracted features 202 include a building corner, a tree-top, a cornerof concrete, a sign corner, and a headlight of a car, as examples ofobject existing around the vehicle 10 that may be image featuressuitable for use as tracked features. The feature extractor 106 may beconfigured to extract features 202 from an image frame until either aspecified number of features have been extracted or features areextracted for a preselected duration of time.

Image frame 200 b shows an image captured subsequent to image frame 200a. In this image frame 200 b, the feature extractor 106 again attemptsto extract the previously identified features, which are now representedas tracked features 204. In this example, the feature extractor 106 wasable to track and extract the building corner, the treetop, and theheadlight. Those track features 204 appeared and were extractable withinthe bounded regions (an example of which is shown at 214) around thelocations predicted by the feature tracker 104. Thus, the featuretracker 104 will update the associated feature tracks for these features204 with the updated position of each of these identified imagefeatures. However, as indicated at 208, two of the extracted features202 previously identified in image frame 200 a are now considered lostfeatures because they no longer can be identified and extracted fromimage frame 200 b. In this case, the two lost features 208 in imageframe 200 b are no longer visible, possibly because of either anobstruction, as in the sign corner, or due to the feature leaving theimage frame, as in the corner of the concrete, or for another reason. Insome embodiments, the feature extractor 106 may decide to identify andextract two additional image features (shown at 206) to replace the lostfeatures 208 (for example, a second sign corner to replace the firstsign corner, and a car tire to replace the corner of the concrete).

Image frame 200 c in FIG. 2A further depicts another image framecaptured subsequent to image frame 200 b. In this image 200 c, thefeature extractor 106 identifies and extracts the previously identifiedfeatures 204 that were present in frame 200 b. Those track features 204appeared and were extractable within the bounded regions (an example ofwhich is shown at 216) around the locations predicted by the featuretracker 104. Two of the features previously tracked in image frame 200 bare now lost features 208 in image frame 200 c. In this example, thelost features 208 are still in the frame but were removed as features bythe feature extractor 106 due to their proximity to the edge of theimage frame, and the availability of more suitable features 206.

In FIG. 2B, the image frame shown at 200 a is the same as shown in FIG.2A. Image frames 201 a and 201 b illustrate subsequently captured imageframes where environmental conditions have changed such that one or moreof the extracted features 202 clearly identifiable in image frame 200 aare no longer sufficiently identifiable for extraction.

For example, in image frame 201 a, the feature extractor 106 attempts toextract the previously identified features 202. Those image features 202that remain extractable in frame 201 a are now represented as trackedfeatures 204. However, an adverse change in environmental conditions hasoccurred that impedes the continued tracking of one or more of thosepreviously tracked features 202. Adverse changes in environmentalconditions may include, for example, changes in lighting due toreflections, glare, shadows, cloud cover, setting of the sun, rain,snow, fog, or other environmental factors. For example, several of thepreviously tracked image features, which are expected to appear atlocations 210, have become undetectable by the feature extractor 106(for example, because of the change in lighting conditions). In thisparticular example, the bounded areas 214, 216 around the image features210 have become too saturated for the recognition of image features. Oneexample of an expected but undetectable feature location 210 is locatedat the top of a tree, where the image feature has become effectivelyindistinguishable from the background sky. Similarly, the headlight of acar becomes an undetectable feature at location 210, where it isindistinguishable from the white body of the car. These image featuresat locations 210 under nominal conditions should have still beidentifiable and extractable by the feature extractor 106 because theystill appear in the image sensor 102 field of view and are not blockedby an intervening object, but none-the-less remain non-extractable fromthe image as captured.

Image frame 201 b in FIG. 2B shows an image captured subsequent to imageframe 201 a. This example image frame 201 b, depicts how a change inlighting conditions can also render image features located within alarge portion of the image frame indistinguishable for the purposes ofsearching and extracting a feature. For example, the sky generallyaround 216 in the image frame 201 b is washed out due to the lightingconditions. The several potentially trackable image features in the skywould be visible if not for the adverse change in lighting conditions.

As discussed above, with embodiments of the present disclosure, when atracked feature no longer appears where expected, image enhancementtechniques are applied to the area of the image from where the trackedfeature was predicted to appear. FIG. 2C illustrates application ofthese enhancements by the dynamic localized parameter adjuster 110 ontoimage frames 201 a and 201 b.

In FIG. 2C, the image frame 200 a is again depicting an image capturedunder nominal environmental conditions. However, in FIG. 2C, the imageframes 201 a and 201 b from FIG. 2B have now been processed by thedynamic localized parameter adjuster 110 as described above to enhancethe ability of the feature extractor 106 to identify and extract whatwere previously undetectable features 210.

The bounded regions 214, 215, and 216 shown in FIG. 2C define the areaswithin the image frames 201 a, 201 b where the feature extractor 106expected associated tracked features 204 to appear within based on theirprior feature tracks and the motion of the vehicle 10 to a new positionand angular orientation since the prior image frames were captured. Insome embodiments, these bounded regions 214, 215, and 216 are also usedto define subsections of the captured image frames where the dynamiclocalized parameter adjuster 110 adjusts one or more imagecharacteristics to attempt to distinguish the missing image features toa sufficient degree for the feature extractor 106 to again be able toextract them.

As an example, in image frame 200 a, the top of a tree (shown at 220)was extracted as an image feature 202. In the subsequent image frame 201a, the top of the tree at 220 could initially no longer be identifieddue to a change in lighting conditions as was shown in FIG. 2B. In FIG.2C, one or more image parameters of the bounded region 214 thatsurrounds the top of the tree at 220 have been adjusted, allowing theimage feature to now be an identified, extracted, and tracked image 204within image frame 201 a. In image frame 201 b, the bounded region 216is smaller than the bounded region 214 in the prior image frame 201 abecause the image feature 202's position could be determined moreaccurately. The success of identifying an obscured feature can beincreased in many ways including, but not limited to, the duration ofthe feature track, the consistency of the of the feature position in theimage frame, the relative motion of the feature to the moving vehicle inprior image frames, or the degree of contrast between pixels of thefeature. As another example, a similar augmentation was applied to thebounded region 215 to an undetectable feature 210 (the headlight of thecar as shown at 221).

In the image from 201 b captured subsequent to image frame 201 a, thebounded region 216 for enhancing the top of the tree at 220 is nowsmaller than the bounded region 214 utilized for that same feature inimage frame 201 a. As discussed above, in some embodiments, the size ofthe bounded regions may be determined as a function of many factors. Forexample, in the image 201 b, the top of the tree identified as an imagefeature 202 in the image frame 200 a doesn't change position greatlybetween image frames. Because of this, the bounded region 216 could besmaller based on the increased confidence of the feature position and/orthe decreased variance of the estimated location of the feature. Inother implementations, the bounded region 216 could be smaller becausethe top of the tree at 220 has been categorized by the feature extractor104 as a static object and/or because the speed of the vehicle 10 hasreduced relative to the top of the tree.

FIG. 3 illustrates an example embodiment of a method 300 for visionaided navigation. It should be understood that the features and elementsdescribed herein with respect to the method shown in FIG. 3 and theaccompanying description may be used in conjunction with, in combinationwith, or substituted for elements of any of the other embodimentsdiscussed herein, and vice versa. Further, it should be understood thatthe functions, structures, and other description of elements associatedwith embodiments of FIG. 3 may apply to like named or described elementsfor any of the Figures and embodiments and vice versa.

In some embodiments, the method 300 may be executed as one or morealgorithms by one or more components of the vision aided navigationsystem 100. Further, in some embodiments, the method 300 is performed asan iterative process using reference to information from prioriterations of the method. Thus, it should be understood that differentidentifiers can be used to describe the same piece of information atdifferent points in the iterative process. Conversely, it should beunderstood that one identifier can be used to describe different piecesof information at different points in the iterative process.

Method 300 begins at 302 with receiving a first image frame. In someembodiments, an image sensor device collects visual information from asurrounding environment and produces a streaming output of image frames.In some embodiments, the image frame is a two-dimensional pixelatedimage where the pixels of the image frame are identifiable viacoordinates and can be referenced in relation to prior or subsequentimage frames. In other embodiments, image frames may represent featuresin other ways, such as through vector coordinates.

The method 300 proceeds to 304 with extracting one or more imagefeatures from the first image frame. The image features extracted at 304are features having characteristics relative to their background such asto be recognizable and distinguishable from their background as anobject that exists in the environment of the vehicle 10. In someexamples, a feature extraction algorithm, e.g. ORB, SIFT, SURF oranother feature extraction algorithm, may be used. Once the imagefeatures are identified, the image features are extracted from theimage. Extracting an image feature consists of identifying a region ofthe image frame as a potential image feature, labeling the region as animage feature, and adding the image feature to a stored list of imagefeatures. In some embodiments, this step may be repeated until all thepossible image features have been extracted. In other embodiments, thisstep repeats until either the number of image features stored in thelist equals a predetermined number of image features or for a presetduration of time.

The method 300 proceeds to 306 with receiving a second image frame. Thesecond image is received from an image sensor device at a point in timeafter the first image is captured at 302.

For example, the second image should be captured within a period of timesuch that featured extracted in the first image may still be expected toappear in the second image given the vehicle's rate and direction ofmotion.

The method proceeds to 308 with estimating a position of the imagefeatures from the first image frame in the second image frame. Anextracted image feature has an associated feature track which contains alist of the image feature's positions, as well as the mean and varianceof the feature's positions, in prior image frames. The feature track canbe used to estimate the position statistics of the image feature basedon the relative motion of the image sensor device and an image featurebetween image frames, thus the position statistics of the image featurecan be predicted in the second frame. In some examples, the featuretrack is used in conjunction with the estimated kinematic states of thevehicle to predict the position statistics of the image feature based onthe motion and position of the vehicle in relation to the image featurein prior image frames. In some examples, the position of the imagefeature is estimated as within the area of the bounded region, asdiscussed above. This bounded region is an area within which the imagefeature is predicted to be located based on the estimated statistics ofthe feature track and the kinematic states of the vehicle.

The method proceeds to 310 with searching for the features in the secondimage frame within a bounded region around the estimated position ofeach image feature. As each feature has certain characteristics, thesecharacteristics can be used to determine whether an identified featureis the image feature.

The method proceeds to 312 with determining if the expected imagefeature has been identified within the bounded region. If the imagefeature has been identified within the proximity area, then the processcontinues proceeds to 316. If the image feature has not been identifiedwithin the bounded region, then the process proceeds to 314 withadjusting the parameters of the image within the bounded region.

At 314, the method comprises adjusting the parameters of the imagewithin the bounded region around the estimated position of the feature.The image frame has certain inherent parameters that can be adjusted oraugmented, for example, luminance, contrast, and gamma. The parametersof the bounded region can be augmented as can the parameters of theimage frame. In some embodiments, the bounded region has a fixed sizeand shape, consistent for every feature. In other embodiments, the sizeand shape of the bounded region can be dynamically adjusted. There areseveral factors that can contribute to the dynamic adjustment of thebounded region including the position of the feature within the imageframe, the relative motion of the feature within the plurality of imageframes, and the feedback from the navigation filter. The position of thefeature within the image frame can be influenced by the expected motionof the vehicle. For example, when a vehicle is moving in a constantdirection with a constant speed, a feature in the middle of the imageframe is more likely to move few pixels than a feature on the edge ofthe image frame. Similarly, if a feature has a relatively large changein location between successive image frames, then the feature is morelikely to change position in a subsequent image frame.

The method proceeds to 316 with updating a feature track for the imagefeature. When an image feature is identified within the bounded region,the image feature's position is added to the feature track. In addition,the feature track is updated to include a track of the relative motionof the image feature progressively through the previous image frames. Ifan image feature was not found within the proximity area, then thecorresponding feature track is removed and the image feature is nolonger in the current list of tracked image features.

In some embodiments, if a feature has not been identified after a numberof iterations of attempting to adjust the image, then the process maycontinue to 316 with the image feature removed from the image featurelist. There are many reasons that an image feature may not be found. Theimage feature could be obscured by another object in the image frame.The image feature could be outside of the bounded region. As well, theimage feature could have simply departed the image frame entirely. If animage feature has been removed during this step, then in the subsequentiteration a new feature is extracted to replace the removed feature. Insome embodiments, the number of extracted features is not constant andmore or fewer features may be tracked in any given iteration.

As well, a navigation filter can provide feedback to influence thebounded region. As discussed in 318, the navigation filter receives afeature track associated with each filter. The navigation filter, or aseparate system, can use the feature tracks to estimate the position andvelocity statistics of the vehicle. The estimated position and velocitystatistics of the vehicle can be used to estimate the positionstatistics of a feature.

The method continues to 318, with providing the updated feature tracksto a navigation filter. Once a requisite number of tracked features havebeen identified and their feature tracks updated, then the latestposition statistics and the feature track of the tracked features areprovided to the navigation filter as a navigation aid for calculating anavigation solution for the vehicle. The updated feature tracks are usedto estimate the position and velocity statistics of the vehicle.

Example Embodiments

Example 1 includes a vision aided navigation system, the systemcomprising: at least one image sensor, wherein the at least one imagesensor is configured to produce a plurality of image frames of asurrounding environment; a feature extractor coupled to the at least oneimage sensor, wherein the feature extractor is configured to extract atleast one image feature from a first image frame from the plurality ofimage frames captured by the at least one image sensor; a navigationfilter configured to output a navigation solution based on an input ofnavigation data from at least one navigation device and further based onchanges in position of the at least one image feature in the pluralityof images; a feature tracker configured to receive the plurality ofimage frames and configured to predict a location of the at least oneimage feature in at least one subsequent image frame taken by the atleast one image sensor; a dynamic localized parameter adjusterconfigured to adjust at least one image parameter of the at least onesubsequent image frame; and wherein the feature tracker is configured sothat when the at least one image feature cannot be identified in the atleast one subsequent image frame within a bounded region around thepredicted location, the dynamic localized parameter adjuster adjusts theat least one image parameter within the bounded region.

Example 2 includes the system of Example 1, wherein the at least oneimage parameter comprises at least one of luminance, contrast, andgamma.

Example 3 includes the system of any of Examples 1-2, wherein the atleast one navigation device comprises at least one of an inertialmeasurement unit or a global navigation satellite system receiver.

Example 4 includes the system of any of Examples 1-3, further comprisinga processor coupled to a memory, wherein at least one of the featureextractor, the feature tracker, the dynamic localized parameteradjuster, or the navigation filter are executed by the processor.

Example 5 includes the system of Example 4, wherein the plurality ofimage frames are stored in the memory.

Example 6 includes the system of any of Examples 4-5, wherein a featuretrack is associated with the at least one image feature, wherein thefeature track comprises a location of the at least one image feature ineach of the plurality of images, and wherein the feature track is storedin the memory.

Example 7 includes the system of Example 6, wherein when the at leastone image feature cannot be identified in the subsequent image after thedynamic localized parameter adjuster adjusts the at least one imageparameter, then the feature track of the at least one image feature isdeleted from the memory, and at least one new image feature is extractedto replace the at least one image feature.

Example 8 includes the system of any of Examples 1-7, wherein thebounded region can be either a predefined area or a dynamically adjustedarea as a function of at least one of: the predicted location of the atleast one image feature in the at least one subsequent image frame; afeedback from the navigation filter; or a change in position of the atleast one image feature across the plurality of image frames.

Example 9 includes the system of any of Examples 6-7, wherein thebounded region comprises an area determined as a function of one or moreof: the feature track, the location of the at least one image feature, avariance of the predicted location of the image feature position, and achange in the location of the at least one image feature in theplurality of image frames.

Example 10 includes the system of any of Examples 1-9, wherein thenavigation solution is for a vehicle comprising one of an automobile, anairplane, a seaworthy vessel, a train, a vertical takeoff and landingvehicle, or a submersible vehicle.

Example 11 includes the system of any of Examples 1-10, wherein thefeature tracker is configured to predict the location of the at leastone image feature in the plurality of image frames as a function of alocation of the at least one image feature in the first image frame anda change in the navigation solution estimated by the navigation filter.

Example 12 includes a method for a vision aided navigation system, themethod comprising: receiving a first image frame; extracting at leastone image feature from the first image frame; receiving a second imageframe; estimating a location of the at least one image feature in thesecond image frame; searching for the at least one image feature withina bounded region around the estimated location of the at least one imagefeature; the at least one image feature cannot be identified, adjustingat least one image parameter within the bounded region around theestimated location of the at least one image feature; updating a featuretrack associated with the at least one image feature; and providing thefeature track to a navigation filter.

Example 13 includes the method of Example 12, wherein the feature trackcomprises a location of the at least one feature in each of theplurality of image frames.

Example 14 includes the method of any of Examples 12-13, wherein thebounded region can be either a predefined area or a dynamically adjustedarea as a function of at least one of: the location of the at least onefeature in the second image frame; a feedback from the navigationfilter; or a change in location of the feature across a plurality ofimage frames.

Example 15 includes the method of any of Examples 12-14, wherein the atleast one image parameter comprises at least one of luminance, contrast,and gamma.

Example 16 includes the method of any of Examples 12-15, wherein whenthe at least one image feature cannot be identified after the at leastone image parameter has been adjusted, removing the feature trackassociated with the at least one image feature and extracting at leastone new image feature.

Example 17 includes the method of any of Examples 12-16 furthercomprising, determining a position and a velocity from the featuretracks associated with the at least one image feature.

Example 18 includes a vision aided navigation system, the systemcomprising: a processor coupled to a memory, wherein the processorexecutes code to implement: a feature extractor coupled to at least oneimage sensor, wherein the at least one image sensor outputs a pluralityof image frames that capture a surrounding environment, wherein thefeature extractor is configured to extract at least one image featurefrom a first image frame of the plurality of image frames taken by theat least one image sensor; a feature tracker configured to predict alocation of the at least one image feature in a second image frame fromthe plurality of image frames taken by the at least one image sensor asa function of the location of the at least one image feature in thefirst image frame and a navigation solution estimated by a navigationfilter, wherein the navigation filter is configured to estimate thenavigation solution based on an input of navigation data from at leastone navigation device and tracking of the location of the at least oneimage feature between the first image frame and the second image frame;and a dynamic localized parameter adjuster configured to adjust at leastone image parameter of the second image frame; and wherein the featuretracker is configured so that when the at least one image feature cannotbe identified in the second image frame within a bounded region aroundthe predicted location, the dynamic localized parameter adjuster adjuststhe at least one image parameter within the bounded region.

Example 19 includes the system of Example 18, wherein the at least oneimage parameter comprises at least one of luminance, contrast, andgamma.

Example 20 includes the system of any of Examples 18-19, wherein afeature track is associated with the at least one image feature, whereinthe feature track comprises the location of the at least one feature ineach of the plurality of image frames, and wherein the feature track isprovided as an input to the navigation filter.

In various alternative embodiments, system and/or device elements,method steps, or example implementations described throughout thisdisclosure (such as any of the vision aided navigation system, orsub-parts of any thereof, for example) may be implemented at least inpart using one or more computer systems, field programmable gate arrays(FPGAs), or similar devices comprising a processor coupled to a memoryand executing code to realize those elements, steps, processes, orexamples, said code stored on a non-transient hardware data storagedevice. Therefore, other embodiments of the present disclosure mayinclude elements comprising program instructions resident on computerreadable media which when implemented by such computer systems, enablethem to implement the embodiments described herein. As used herein, theterm “computer readable media” refers to tangible memory storage deviceshaving non-transient physical forms. Such non-transient physical formsmay include computer memory devices, such as but not limited to punchcards, magnetic disk or tape, any optical data storage system, flashread only memory (ROM), non-volatile ROM, programmable ROM (PROM),erasable-programmable ROM (E-PROM), random access memory (RAM), or anyother form of permanent, semi-permanent, or temporary memory storagesystem or device having a physical, tangible form. Program instructionsinclude, but are not limited to, computer-executable instructionsexecuted by computer system processors and hardware descriptionlanguages such as Very High-Speed Integrated Circuit (VHSIC) HardwareDescription Language (VHDL).

As used herein, terms such as “processor”, “image sensor”, “navigationunit”, “inertial measurement unit”, “GNSS receiver, each refer tonon-generic device elements of a navigation system that would berecognized and understood by those of skill in the art and are not usedherein as nonce words or nonce terms for the purpose of invoking 35 USC112(f).

Although specific embodiments have been illustrated and describedherein, it will be appreciated by those of ordinary skill in the artthat any arrangement, which is calculated to achieve the same purpose,may be substituted for the specific embodiments shown. Therefore, it ismanifestly intended that this invention be limited only by the claimsand the equivalents thereof.

1. A vision aided navigation system, the system comprising: at least oneimage sensor, wherein the at least one image sensor is configured toproduce a plurality of image frames of a surrounding environment; afeature extractor coupled to the at least one image sensor, wherein thefeature extractor is configured to extract at least one image featurefrom a first image frame from the plurality of image frames captured bythe at least one image sensor; a navigation filter configured to outputa navigation solution based on an input of navigation data from at leastone navigation device and further based on changes in position of the atleast one image feature in the plurality of images; a feature trackerconfigured to receive the plurality of image frames and configured topredict a location of the at least one image feature in at least onesubsequent image frame taken by the at least one image sensor; and adynamic localized parameter adjuster configured to adjust at least oneimage parameter of the at least one subsequent image frame; wherein afeature track is associated with the at least one image feature, whereinthe feature track comprises a location of the at least one image featurein each of the plurality of images, and wherein the feature track isstored in the memory; wherein the feature tracker is configured so thatwhen the at least one image feature cannot be identified in the at leastone subsequent image frame within a bounded region around the predictedlocation, the dynamic localized parameter adjuster adjusts the at leastone image parameter within the bounded region; wherein when the at leastone image feature cannot be identified in the subsequent image after thedynamic localized parameter adjuster adjusts the at least one imageparameter, then the feature track of the at least one image feature isdeleted from the memory, and at least one new image feature is extractedto replace the at least one image feature.
 2. The system of claim 1,wherein the at least one image parameter comprises at least one ofluminance, contrast, and gamma.
 3. The system of claim 1, wherein the atleast one navigation device comprises at least one of an inertialmeasurement unit or a global navigation satellite system receiver. 4.The system of claim 1, further comprising a processor coupled to amemory, wherein at least one of the feature extractor, the featuretracker, the dynamic localized parameter adjuster, or the navigationfilter are executed by the processor.
 5. The system of claim 1, whereinthe plurality of image frames are stored in the memory. 6-7. (canceled)8. The system of claim 1, wherein the bounded region can be either apredefined area or a dynamically adjusted area as a function of at leastone of: the predicted location of the at least one image feature in theat least one subsequent image frame; a feedback from the navigationfilter; or a change in position of the at least one image feature acrossthe plurality of image frames.
 9. The system of claim 1, wherein thebounded region comprises an area determined as a function of one or moreof: the feature track, the location of the at least one image feature, avariance of the predicted location of the image feature position, and achange in the location of the at least one image feature in theplurality of image frames.
 10. The system of claim 1, wherein thenavigation solution is for a vehicle comprising one of an automobile, anairplane, a seaworthy vessel, a train, a vertical takeoff and landingvehicle, or a submersible vehicle.
 11. The system of claim 1, whereinthe feature tracker is configured to predict the location of the atleast one image feature in the plurality of image frames as a functionof a location of the at least one image feature in the first image frameand a change in the navigation solution estimated by the navigationfilter. 12-17. (canceled)
 18. A vision aided navigation system, thesystem comprising: a processor coupled to a memory, wherein theprocessor executes code to implement: a feature extractor coupled to atleast one image sensor, wherein the at least one image sensor outputs aplurality of image frames that capture a surrounding environment,wherein the feature extractor is configured to extract at least oneimage feature from a first image frame of the plurality of image framestaken by the at least one image sensor; a feature tracker configured topredict a location of the at least one image feature in a second imageframe from the plurality of image frames taken by the at least one imagesensor as a function of the location of the at least one image featurein the first image frame and a navigation solution estimated by anavigation filter, wherein the navigation filter is configured toestimate the navigation solution based on an input of navigation datafrom at least one navigation device and tracking of the location of theat least one image feature between the first image frame and the secondimage frame; and a dynamic localized parameter adjuster configured toadjust at least one image parameter of the second image frame byapplying an image correction filter; wherein a feature track isassociated with the at least one image feature, wherein the featuretrack comprises the location of the at least one image feature in eachof the plurality of image frames, and wherein the feature track isstored in the memory; wherein the feature tracker is configured so thatwhen the at least one image feature cannot be identified in the secondimage frame within a bounded region around the predicted location, thedynamic localized parameter adjuster adjusts the at least one imageparameter within the bounded region by applying the image correctionfilter; wherein the bounded region comprises an area determined as afunction of one or more of: the feature track, the location of the atleast one image feature, a variance of a predicted location of an imagefeature position, and a change in the location of the at least one imagefeature in the plurality of image frames.
 19. The system of claim 18,wherein the at least one image parameter comprises at least one ofluminance, contrast, and gamma.
 20. The system of claim 18, wherein thefeature track is provided as an input to the navigation filter.